#include "car.hpp"
#include <glog/logging.h>

car::car(zdt_motor *wheel1, zdt_motor *wheel2,
         zdt_motor *wheel3, zdt_motor *wheel4,
         zdt_motor *rotate_motor, zdt_motor *lift_motor,
         zdt_motor *bracket_motor, pwm_board *claw_board,
         uint32_t Rx, uint32_t Ry, double wheel_d,
         code_scan *scanner, tjc_screen *screen)
    : _wheel1(wheel1), _wheel2(wheel2),
      _wheel3(wheel3), _wheel4(wheel4),
      _rotate_motor(rotate_motor),
      _lift_motor(lift_motor),
      _bracket_motor(bracket_motor),
      _claw_board(claw_board),
      _Rx(Rx), _Ry(Ry), _wheel_d(wheel_d),
      _dest_car_x(0), _dest_car_y(0), _dest_car_angle(0),
      _scanner(scanner), _screen(screen)
{
}
yj_return car::_cal_wheel_speed(const car_speed_t &car_speed, car_wheel_speed_t &wheel_speed)
{
    double pra1 = 60 / M_PI / _wheel_d; // mm/s -> rpm
    double pra2 = 0.001 * 2 * M_PI;     // 0.001 r/s → rad/s

    wheel_speed.wheel1_speed = car_speed.x_speed * pra1 - car_speed.y_speed * pra1 - car_speed.z_speed * pra2 * (_Rx + _Ry);
    wheel_speed.wheel2_speed = car_speed.x_speed * pra1 + car_speed.y_speed * pra1 + car_speed.z_speed * pra2 * (_Rx + _Ry);
    wheel_speed.wheel3_speed = car_speed.x_speed * pra1 + car_speed.y_speed * pra1 - car_speed.z_speed * pra2 * (_Rx + _Ry);
    wheel_speed.wheel4_speed = car_speed.x_speed * pra1 - car_speed.y_speed * pra1 + car_speed.z_speed * pra2 * (_Rx + _Ry);
    wheel_speed.motor_aceel = car_speed.motor_aceel;

    _limit_wheel_min_speed(wheel_speed);

    // LOG(INFO) << "in2 wheel1_speed:" << wheel_speed.wheel1_speed << " wheel2_speed:" << wheel_speed.wheel2_speed
    //           << " wheel3_speed:" << wheel_speed.wheel3_speed << " wheel4_speed:" << wheel_speed.wheel4_speed;
    return YJ_OK;
}

yj_return car::cal_car_speed(const car_wheel_speed_t &wheel_speed, car_speed_t &car_speed)
{
    double pra1 = M_PI * _wheel_d / 60; // rpm -> mm/s
    double pra2 = 0.001 * 2 * M_PI;     // 0.001 r/s → rad/s

    car_speed.x_speed = (wheel_speed.wheel1_speed + wheel_speed.wheel2_speed + wheel_speed.wheel3_speed + wheel_speed.wheel4_speed) * pra1 / 4;
    car_speed.y_speed = (wheel_speed.wheel2_speed + wheel_speed.wheel3_speed - wheel_speed.wheel1_speed - wheel_speed.wheel4_speed) * pra1 / 4;
    car_speed.z_speed = (wheel_speed.wheel2_speed + wheel_speed.wheel4_speed - wheel_speed.wheel1_speed - wheel_speed.wheel3_speed) * pra2 / 4 / (_Rx + _Ry);
    car_speed.motor_aceel = 0;
    return YJ_OK;
}

void car::_limit_wheel_min_speed(car_wheel_speed_t &wheel_speed)
{
    double min_speed = 0;
    if (wheel_speed.wheel1_speed < 1 && wheel_speed.wheel1_speed > 0)
    {
        min_speed = (wheel_speed.wheel1_speed < min_speed) ? wheel_speed.wheel1_speed : min_speed;
    }
    if (wheel_speed.wheel2_speed < 1 && wheel_speed.wheel2_speed > 0)
    {
        min_speed = (wheel_speed.wheel2_speed < min_speed) ? wheel_speed.wheel2_speed : min_speed;
    }
    if (wheel_speed.wheel3_speed < 1 && wheel_speed.wheel3_speed > 0)
    {
        min_speed = (wheel_speed.wheel3_speed < min_speed) ? wheel_speed.wheel3_speed : min_speed;
    }
    if (wheel_speed.wheel4_speed < 1 && wheel_speed.wheel4_speed > 0)
    {
        min_speed = (wheel_speed.wheel4_speed < min_speed) ? wheel_speed.wheel4_speed : min_speed;
    }

    if (min_speed == 0)
        return;

    wheel_speed.wheel1_speed /= min_speed;
    wheel_speed.wheel2_speed /= min_speed;
    wheel_speed.wheel3_speed /= min_speed;
    wheel_speed.wheel4_speed /= min_speed;
    if (min_speed < 0)
    {
        wheel_speed.wheel1_speed *= -1;
        wheel_speed.wheel2_speed *= -1;
        wheel_speed.wheel3_speed *= -1;
        wheel_speed.wheel4_speed *= -1;
    }
}
yj_return car::set_speed(double x_speed, double y_speed, double z_speed, int32_t motor_aceel)
{
    car_speed_t car_speed_temp;
    car_wheel_speed_t wheel_speed_temp;
    car_speed_temp.x_speed = x_speed;
    car_speed_temp.y_speed = y_speed;
    car_speed_temp.z_speed = z_speed;
    car_speed_temp.motor_aceel = motor_aceel;

    _cal_wheel_speed(car_speed_temp, wheel_speed_temp);

    // LOG(INFO) << "wheel1_speed:" << wheel_speed_temp.wheel1_speed
    //           << " wheel2_speed:" << wheel_speed_temp.wheel2_speed
    //           << " wheel3_speed:" << wheel_speed_temp.wheel3_speed
    //           << " wheel4_speed:" << wheel_speed_temp.wheel4_speed;

    int retry = 0;
    yj_return flag;

    do
    {
        if (_wheel1->set_speed(static_cast<int32_t>(wheel_speed_temp.wheel1_speed), wheel_speed_temp.motor_aceel, true) == YJ_OK &&
            _wheel2->set_speed(static_cast<int32_t>(wheel_speed_temp.wheel2_speed), wheel_speed_temp.motor_aceel, true) == YJ_OK &&
            _wheel3->set_speed(static_cast<int32_t>(wheel_speed_temp.wheel3_speed), wheel_speed_temp.motor_aceel, true) == YJ_OK &&
            _wheel4->set_speed(static_cast<int32_t>(wheel_speed_temp.wheel4_speed), wheel_speed_temp.motor_aceel, true) == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    retry = 0;
    if (flag == YJ_OK)
        do
        {
            if (_wheel1->start_sync(true) == YJ_OK &&
                _wheel3->start_sync(true) == YJ_OK)
                return YJ_OK;
        } while (retry++ < 3);

    LOG(ERROR) << "car set speed error";
    return YJ_ERROR;
}

yj_return car::set_speed(const car_speed_t &car_speed)
{
    car_wheel_speed_t wheel_speed_temp;
    _cal_wheel_speed(car_speed, wheel_speed_temp);

    int retry = 0;
    yj_return flag;
    do
    {
        if (_wheel1->set_speed(wheel_speed_temp.wheel1_speed, wheel_speed_temp.motor_aceel, true) == YJ_OK &&
            _wheel2->set_speed(wheel_speed_temp.wheel2_speed, wheel_speed_temp.motor_aceel, true) == YJ_OK &&
            _wheel3->set_speed(wheel_speed_temp.wheel3_speed, wheel_speed_temp.motor_aceel, true) == YJ_OK &&
            _wheel4->set_speed(wheel_speed_temp.wheel4_speed, wheel_speed_temp.motor_aceel, true) == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    retry = 0;
    if (flag == YJ_OK)
        do
        {
            if (_wheel1->start_sync(true) == YJ_OK &&
                _wheel3->start_sync(true) == YJ_OK)
                return YJ_OK;
        } while (retry++ < 3);

    LOG(ERROR) << "car set speed error";
    return YJ_ERROR;
}

yj_return car::read_wheel_speed(car_wheel_speed_t &wheel_speed)
{
    int32_t temp1, temp2, temp3, temp4;
    if (_wheel1->read_speed(temp1) == YJ_OK &&
        _wheel2->read_speed(temp2) == YJ_OK &&
        _wheel3->read_speed(temp3) == YJ_OK &&
        _wheel4->read_speed(temp4) == YJ_OK)
    {
        wheel_speed.wheel1_speed = -temp1; // 电机安装方向导致的反向
        wheel_speed.wheel2_speed = temp2;
        wheel_speed.wheel3_speed = -temp3; // 电机安装方向导致的反向
        wheel_speed.wheel4_speed = temp4;
        return YJ_OK;
    }
    else
        return YJ_ERROR;
}

yj_return car::set_rotate_pos(double step_angle,
                              double speed,
                              uint8_t accel,
                              zdt_motor::motor_pos_mode_en pos_mode,
                              bool is_waiting)
{
    int retry = 0;
    yj_return flag;
    double circle_num = step_angle * 20 / 360;
    // rpm<-o/s
    double speed_temp = static_cast<int32_t>(speed * 60 * 20 / 360);

    speed_temp = abs(speed_temp);
    if (circle_num < 0)
    {
        circle_num *= -1;
        speed_temp *= -1;
    }
    do
    {
        if (_rotate_motor->set_position(static_cast<int32_t>(speed_temp), accel, circle_num,
                                        false, pos_mode, is_waiting) == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    return flag;
}

yj_return car::set_rotate_vel(double speed, uint8_t accel)
{
    int retry = 0;
    yj_return flag;
    double speed_temp = static_cast<int32_t>(speed * 60 * 20 / 360);
    do
    {
        if (_rotate_motor->set_speed(static_cast<int32_t>(speed_temp), accel) == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    return flag;
}

yj_return car::set_lift_pos(double step_mm,
                            double speed,
                            uint8_t accel,
                            zdt_motor::motor_pos_mode_en pos_mode,
                            bool is_waiting)
{
    int retry = 0;
    yj_return flag;
    double circle_num = step_mm / 6;
    // rpm<-mm/s
    double speed_temp = static_cast<int32_t>(speed * 60 / 6);

    speed_temp = abs(speed_temp);
    if (circle_num < 0)
    {
        circle_num *= -1;
        speed_temp *= -1;
    }

    do
    {
        if (_lift_motor->set_position(static_cast<int32_t>(speed_temp), accel, circle_num,
                                      false, pos_mode, is_waiting) == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    return flag;
}

yj_return car::set_lift_vel(double speed, uint8_t accel)
{
    int retry = 0;
    yj_return flag;
    double speed_temp = static_cast<int32_t>(speed * 60 / 6);
    do
    {
        if (_lift_motor->set_speed(static_cast<int32_t>(speed_temp), accel) == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    return flag;
}
yj_return car::set_bracket_pos(double step_mm,
                               double speed,
                               uint8_t accel,
                               zdt_motor::motor_pos_mode_en pos_mode,
                               bool is_waiting)
{
    int retry = 0;
    yj_return flag;
    double circle_num = step_mm / 125.6637; // 已更改为20齿齿轮对应圈数
    double speed_temp = static_cast<int32_t>(speed * 60 / 125.6637);
    // mm/s -> rpm
    speed_temp = abs(speed_temp);
    if (circle_num < 0)
    {
        circle_num *= -1;
        speed_temp *= -1;
    }

    do
    {
        if (_bracket_motor->set_position(static_cast<int32_t>(speed_temp), accel, circle_num,
                                         false, pos_mode, is_waiting) == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    return flag;
}

yj_return car::set_bracket_vel(double speed, uint8_t accel)
{
    int retry = 0;
    yj_return flag;
    double speed_temp = static_cast<int32_t>(speed * 60 / 251.3274 / 2);
    do
    {
        if (_bracket_motor->set_speed(static_cast<int32_t>(speed_temp), accel) == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    // LOG(INFO) << "set bracket vel:" << speed_temp;

    return flag;
}

yj_return car::reset_rotate_pos()
{
    int retry = 0;
    yj_return flag;
    do
    {
        if (_rotate_motor->set_zero() == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    if (flag == YJ_ERROR)
    {
        LOG(ERROR) << "reset rotate motor zero error";
        return flag;
    }
    bool is_zero, is_zero_ok;
    retry = 0;
    flag = YJ_ERROR;
    _rotate_motor->clear_protect();
    _rotate_motor->clear_pos();
    while (retry < 3)
    {

        if (_rotate_motor->read_zero_status(is_zero, is_zero_ok) != YJ_OK)
        {
            flag = YJ_ERROR;
            retry++;
            continue;
        }

        if (is_zero != true)
        {
            if (is_zero_ok)
                flag = YJ_OK;
            else
                flag = YJ_ERROR;
            break;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    _rotate_motor->clear_pos();

    return flag;
}
yj_return car::reset_lift_pos()
{
    int retry = 0;
    yj_return flag;
    do
    {
        if (_lift_motor->set_zero() == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    if (flag == YJ_ERROR)
    {
        LOG(ERROR) << "reset rotate motor zero error";
        return flag;
    }
    bool is_zero, is_zero_ok;
    retry = 0;
    flag = YJ_ERROR;
    while (retry < 3)
    {
        _lift_motor->clear_protect();
        _lift_motor->clear_pos();
        if (_lift_motor->read_zero_status(is_zero, is_zero_ok) != YJ_OK)
        {
            flag = YJ_ERROR;
            retry++;
            continue;
        }

        if (is_zero != true)
        {
            if (is_zero_ok)
                flag = YJ_OK;
            else
                flag = YJ_ERROR;
            break;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    _lift_motor->clear_pos();

    return flag;
}
yj_return car::reset_bracket_pos()
{
    int retry = 0;
    yj_return flag;
    do
    {
        if (_bracket_motor->set_zero() == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    if (flag == YJ_ERROR)
    {
        LOG(ERROR) << "reset rotate motor zero error";
        return flag;
    }
    bool is_zero, is_zero_ok;
    retry = 0;
    flag = YJ_ERROR;
    while (retry < 3)
    {
        _bracket_motor->clear_protect();
        _bracket_motor->clear_pos();
        if (_bracket_motor->read_zero_status(is_zero, is_zero_ok) != YJ_OK)
        {
            flag = YJ_ERROR;
            retry++;
            continue;
        }

        if (is_zero != true)
        {
            if (is_zero_ok)
                flag = YJ_OK;
            else
                flag = YJ_ERROR;
            break;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    _bracket_motor->clear_pos();

    return flag;
}

yj_return car::clear_all_wheel_protect_flag()
{
    int retry = 0;
    yj_return flag;
    do
    {
        if (_wheel1->clear_protect() == YJ_OK &&
            _wheel2->clear_protect() == YJ_OK &&
            _wheel3->clear_protect() == YJ_OK &&
            _wheel4->clear_protect() == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    return flag;
}
yj_return car::clear_rotate_protect_flag()
{
    int retry = 0;
    yj_return flag;
    do
    {
        if (_rotate_motor->clear_protect() == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    return flag;
}
yj_return car::clear_lift_protect_flag()
{
    int retry = 0;
    yj_return flag;
    do
    {
        if (_lift_motor->clear_protect() == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    return flag;
}
yj_return car::clear_bracket_protect_flag()
{
    int retry = 0;
    yj_return flag;
    do
    {
        if (_bracket_motor->clear_protect() == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    return flag;
}

yj_return car::set_claw_channel(uint8_t channel, uint8_t value)
{
    int retry = 0;
    yj_return flag;
    do
    {
        if (_claw_board->write_channel(channel, value) == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    return flag;
}

yj_return car::set_claw_led(pwm_board::led_color_en color, uint8_t brightness, pwm_board::led_mode_en mode)
{
    int retry = 0;
    yj_return flag;
    do
    {
        if (_claw_board->set_led(color, brightness, mode) == YJ_OK)
            flag = YJ_OK;
        else
            flag = YJ_ERROR;
    } while (retry++ < 3 && flag != YJ_OK);

    return flag;
}

yj_return car::read_bracket_speed_and_pos(double &speed, double &pos)
{
    int32_t speed_temp;
    double pos_temp;

    if (_bracket_motor->read_speed(speed_temp) == YJ_OK &&
        _bracket_motor->read_position(pos_temp) == YJ_OK)
    {
        // rpm -> mm/s
        speed = -1 * speed_temp * 125.6637 / 60;
        pos = -1 * pos_temp * 125.6637 / 360;

        // LOG(INFO) << "bracket speed: " << speed << " mm/s, pos: " << pos << " mm";
        return YJ_OK;
    }
    else
        return YJ_ERROR;
}

yj_return car::read_lift_speed_and_pos(double &speed, double &pos)
{
    int32_t speed_temp;
    double pos_temp;

    if (_lift_motor->read_speed(speed_temp) == YJ_OK &&
        _lift_motor->read_position(pos_temp) == YJ_OK)
    {
        speed = speed_temp * 6 / 60;
        pos = pos_temp * 6 / 360;
        return YJ_OK;
    }
    else
        return YJ_ERROR;
}
yj_return car::read_rotate_speed_and_pos(double &speed, double &pos)
{
    int32_t speed_temp;
    double pos_temp;

    if (_rotate_motor->read_speed(speed_temp) == YJ_OK &&
        _rotate_motor->read_position(pos_temp) == YJ_OK)
    {
        speed = speed_temp * 360 / 20 / 60;
        pos = pos_temp / 20;
        return YJ_OK;
    }
    else
        return YJ_ERROR;
}

yj_return car::scan_code()
{
    return _scanner->get_code();
}
yj_return car::read_color_index_with_round(uint8_t round_index, uint8_t times_index, uint8_t &color_index)
{
    return _scanner->get_color_index_with_round(round_index, times_index, color_index);
}

bool car::read_start_flag()
{
    return _screen->read_start_flag();
}

yj_return car::get_set_color_sq_screen(tjc_screen::tjc_set_color_en get_witch, uint8_t &get_value)
{
    return _screen->get_set_color_sq_screen(get_witch, get_value);
}